# -*- coding: utf-8 -*-
"""
STP23L激光测距传感器Python 3.10兼容版本
功能：读取STP23L测距传感器的数据并解析
"""
import serial
import time
import threading
import logging
import numpy as np  # 用于统计计算
import pandas as pd  # 用于创建Excel文件
from typing import Optional, Dict, Union
from TOF_Measure import TOF_Measure

# 配置日志
logging.basicConfig(
    level=logging.INFO,
    format="%(asctime)s - %(name)s - %(levelname)s - %(message)s",
    handlers=[logging.StreamHandler()]
)
logger = logging.getLogger("STP23L_Range_Test")


def parse_data(data):
    """
    解析STP23L传感器数据
    
    Args:
        data: 从串口读取的原始字节数据
        
    Returns:
        list: 包含多个测量点的元组列表，每个元组包含(distance, noise, peak, confidence, intg, reftof)
    """
    # 检查起始符
    if data[0] != 0xAA:
        return []  # 静默处理无效数据包

    measurements = []
    
    # 从第11个字节开始解析每个测量数据点
    for i in range(10, 195, 15):
        if i + 15 < len(data):  # 确保不会越界
            #---------------距离数据distance------------------
            distance1 = data[i + 1]
            distance1 = hex(distance1)   # 将十进制整数转换为十六进制字符串表示
            interval1 = distance1[2:].upper()   # 去掉十六进制前面的0x
            str1 = str(interval1)     # 将 interval1 转为字符串
            distance2 = data[i]
            distance2 = hex(distance2)
            interval2 = distance2[2:].upper()
            str2 = str(interval2)
            string1 = str1 + str2           # 拼接高字节和低字节（用字符串来表示）
            distance = int(string1, 16)    # 将一个字符串按照16进制的方式解析并转换为整数

            #------------------环境噪声noise--------------------
            noise1 = data[i + 3] 
            noise1 = hex(noise1)
            noise1_16 = noise1[2:].upper()
            noise1_str = str(noise1_16)
            noise2 = data[i + 2]
            noise2 = hex(noise2)
            noise2_16 = noise2[2:].upper()
            noise2_str = str(noise2_16)
            noise_str = noise1_str + noise2_str
            noise = int(noise_str, 16)

            #-------------------------接收强度信息peak--------------------
            peak1 = data[i + 7]
            peak1 = hex(peak1)
            peak1_16 = peak1[2:].upper()
            peak1_str = str(peak1_16)
            peak2 = data[i + 6]
            peak2 = hex(peak2)
            peak2_16 = peak2[2:].upper()
            peak2_str = str(peak2_16)
            peak3 = data[i + 5]
            peak3 = hex(peak3)
            peak3_16 = peak3[2:].upper()
            peak3_str = str(peak3_16)
            peak4 = data[i + 4]
            peak4 = hex(peak4)
            peak4_16 = peak4[2:].upper()
            peak4_str = str(peak4_16)
            peak_str = peak1_str + peak2_str + peak3_str + peak4_str
            peak = int(peak_str, 16)

            #-------------------------置信度confidence--------------------
            confidence = data[i + 8]

            #-------------------------积分次数intg---------------------
            intg1 = data[i + 12]
            intg1 = hex(intg1)
            intg1_16 = intg1[2:].upper()
            intg1_str = str(intg1_16)
            intg2 = data[i + 11]
            intg2 = hex(intg2)
            intg2_16 = intg2[2:].upper()
            intg2_str = str(intg2_16)
            intg3 = data[i + 10]
            intg3 = hex(intg3)
            intg3_16 = intg3[2:].upper()
            intg3_str = str(intg3_16)
            intg4 = data[i + 9]
            intg4 = hex(intg4)
            intg4_16 = intg4[2:].upper()
            intg4_str = str(intg4_16)
            intg_str = intg1_str + intg2_str + intg3_str + intg4_str
            intg = int(intg_str, 16)

            #--------------------------温度表征值reftof-----------------
            reftof1 = data[i + 14]
            reftof1 = hex(reftof1)
            reftof1_16 = reftof1[2:].upper()
            reftof1_str = str(reftof1_16)
            reftof2 = data[i + 13]
            reftof2 = hex(reftof2)
            reftof2_16 = reftof2[2:].upper()
            reftof2_str = str(reftof2_16)
            reftof_str = reftof1_str + reftof2_str
            reftof = int(reftof_str, 16)
            
            measurements.append((distance, noise, peak, confidence, intg, reftof))
    
    return measurements


def read_serial_data():
    """
    从串口读取STP23L传感器数据并解析，执行50次测量并统计
    """
    try:
        with serial.Serial(port, baudrate, timeout=5) as ser:
            time.sleep(2)  # 等待设备准备
            
            # 创建STP23L实例用于统计
            stp23l = STP23L_RangeFinder(port, baudrate)
            measurement_count = 0
            
            logger.info("开始STP23L测距数据采集，将进行50次测量...")
            
            while measurement_count < stp23l.max_measurements:
                if ser.in_waiting > 0:
                    raw_data = b''
                    while len(raw_data) < 195:
                        # 读取串口中所有可用的数据
                        raw_data += ser.read(ser.in_waiting)
                        time.sleep(0.01)  # 可以加一些延时，避免CPU占用过高
                    
                    # 确保读取到完整的195字节
                    if len(raw_data) >= 195:
                        data = list(bytearray(raw_data[:195]))  # 取前195字节，防止溢出
                        measurements = parse_data(data)
                        
                        if measurements:
                            # 只输出第一个测量点的距离数据
                            distance_mm = measurements[0][0]  # 获取第一个测量点的距离值（毫米）
                            
                            # 添加到测量列表
                            stp23l.add_measurement(distance_mm)
                            measurement_count += 1
                            
                            # 显示当前测量
                            logger.info(f"测量 {measurement_count}/{stp23l.max_measurements}: {distance_mm:.2f} 毫米")
                            
    except serial.SerialException as e:
        logger.error(f"串口错误: {e}")
    except KeyboardInterrupt:
        logger.info("用户中断测量")
    except Exception as e:
        logger.error(f"测量异常: {e}")
    finally:
        # 打印测量结果表格
        stp23l.print_measurement_table()
        
        # 导出Excel文件
        timestamp = time.strftime("%Y%m%d_%H%M%S")
        excel_filename = f"STP23L_Measurements_{timestamp}.xlsx"
        
        if stp23l.export_to_excel(excel_filename):
            logger.info(f"测量数据已导出到Excel文件: {excel_filename}")
        else:
            logger.error("导出Excel文件失败")


class STP23L_RangeFinder(TOF_Measure):
    """STP23L激光测距传感器封装类"""
    
    def __init__(self, port=None, baudrate=230400):
        """
        初始化STP23L测距传感器
        
        Args:
            port: 串口号（默认自动选择）
            baudrate: 波特率
        """
        # 如果没有指定串口号，则根据系统自动选择
        if port is None:
            import sys
            if sys.platform == 'win32':
                self.port = 'COM3'  # Windows系统下的默认串口号
            else:
                self.port = '/dev/ttyACM0'  # Linux系统下的默认串口号
        else:
            self.port = port
            
        self.baudrate = baudrate
        self.ser = None
        self.distance = 0.0  # 最新测距值（毫米）
        self.lock = threading.Lock()
        self.thread = None
        self.running = False
        self.sensor_type = "STP23L"
        
        # 测量数据存储
        self.measurements = []  # 存储所有测量值
        self.max_measurements = 50  # 最大测量次数限制
        
    def open(self) -> bool:
        """
        打开传感器连接
        实现基类的抽象方法
        
        Returns:
            bool: 打开是否成功
        """
        try:
            self.ser = serial.Serial(self.port, self.baudrate, timeout=5)
            time.sleep(2)  # 等待设备准备
            self.is_connected = True
            logger.info(f"STP23L串口已打开：{self.port}（波特率：{self.baudrate}）")
            return True
        except Exception as e:
            logger.error(f"STP23L连接失败: {e}")
            return False
    
    def close(self) -> bool:
        """
        关闭传感器连接
        实现基类的抽象方法
        
        Returns:
            bool: 关闭是否成功
        """
        try:
            self.running = False
            if self.thread:
                self.thread.join(timeout=1)
            
            if self.ser and self.ser.is_open:
                self.ser.close()
                self.is_connected = False
                logger.info(f"STP23L串口{self.port}已关闭")
                return True
            else:
                logger.warning(f"STP23L串口{self.port}已处于关闭状态")
                return True
        except Exception as e:
            logger.error(f"关闭STP23L串口失败：{str(e)}")
            return False
    
    def parse_data(self, data: bytes) -> Optional[Dict[str, Union[float, int]]]:
        """
        解析传感器数据
        实现基类的抽象方法
        
        Args:
            data: 从传感器读取的原始数据
            
        Returns:
            包含distance(mm)等信息的字典，解析失败返回None
        """
        try:
            # 将字节数据转换为列表
            data_list = list(bytearray(data))
            
            # 检查起始符
            if len(data_list) == 0 or data_list[0] != 0xAA:
                logger.warning("无效的数据包，起始符不是0xAA")
                return None
            
            # 从第11个字节开始解析第一个测量数据点
            i = 10
            if i + 15 < len(data_list):  # 确保不会越界
                #---------------距离数据distance------------------
                distance1 = data_list[i + 1]
                distance1 = hex(distance1)   # 将十进制整数转换为十六进制字符串表示
                interval1 = distance1[2:].upper()   # 去掉十六进制前面的0x
                str1 = str(interval1)     # 将 interval1 转为字符串
                distance2 = data_list[i]
                distance2 = hex(distance2)
                interval2 = distance2[2:].upper()
                str2 = str(interval2)
                string1 = str1 + str2           # 拼接高字节和低字节（用字符串来表示）
                distance = int(string1, 16)    # 将一个字符串按照16进制的方式解析并转换为整数

                #------------------环境噪声noise--------------------
                noise1 = data_list[i + 3] 
                noise1 = hex(noise1)
                noise1_16 = noise1[2:].upper()
                noise1_str = str(noise1_16)
                noise2 = data_list[i + 2]
                noise2 = hex(noise2)
                noise2_16 = noise2[2:].upper()
                noise2_str = str(noise2_16)
                noise_str = noise1_str + noise2_str
                noise = int(noise_str, 16)

                #-------------------------接收强度信息peak--------------------
                peak1 = data_list[i + 7]
                peak1 = hex(peak1)
                peak1_16 = peak1[2:].upper()
                peak1_str = str(peak1_16)
                peak2 = data_list[i + 6]
                peak2 = hex(peak2)
                peak2_16 = peak2[2:].upper()
                peak2_str = str(peak2_16)
                peak3 = data_list[i + 5]
                peak3 = hex(peak3)
                peak3_16 = peak3[2:].upper()
                peak3_str = str(peak3_16)
                peak4 = data_list[i + 4]
                peak4 = hex(peak4)
                peak4_16 = peak4[2:].upper()
                peak4_str = str(peak4_16)
                peak_str = peak1_str + peak2_str + peak3_str + peak4_str
                peak = int(peak_str, 16)

                #-------------------------置信度confidence--------------------
                confidence = data_list[i + 8]

                #-------------------------积分次数intg---------------------
                intg1 = data_list[i + 12]
                intg1 = hex(intg1)
                intg1_16 = intg1[2:].upper()
                intg1_str = str(intg1_16)
                intg2 = data_list[i + 11]
                intg2 = hex(intg2)
                intg2_16 = intg2[2:].upper()
                intg2_str = str(intg2_16)
                intg3 = data_list[i + 10]
                intg3 = hex(intg3)
                intg3_16 = intg3[2:].upper()
                intg3_str = str(intg3_16)
                intg4 = data_list[i + 9]
                intg4 = hex(intg4)
                intg4_16 = intg4[2:].upper()
                intg4_str = str(intg4_16)
                intg_str = intg1_str + intg2_str + intg3_str + intg4_str
                intg = int(intg_str, 16)

                #--------------------------温度表征值reftof-----------------
                reftof1 = data_list[i + 14]
                reftof1 = hex(reftof1)
                reftof1_16 = reftof1[2:].upper()
                reftof1_str = str(reftof1_16)
                reftof2 = data_list[i + 13]
                reftof2 = hex(reftof2)
                reftof2_16 = reftof2[2:].upper()
                reftof2_str = str(reftof2_16)
                reftof_str = reftof1_str + reftof2_str
                reftof = int(reftof_str, 16)
                
                # 更新最新测距值
                self.distance = distance
                
                # 返回解析结果
                result = {
                    "distance": distance,  # 距离（毫米）
                    "noise": noise,  # 环境噪声
                    "peak": peak,  # 接收强度信息
                    "confidence": confidence,  # 置信度
                    "intg": intg,  # 积分次数
                    "reftof": reftof  # 温度表征值
                }
                logger.debug(f"STP23L数据解析结果：{result}")
                return result
            else:
                logger.warning("数据长度不足，无法解析")
                return None
        except Exception as e:
            logger.error(f"解析STP23L数据失败：{str(e)}")
            return None
    
    def get_distance(self) -> Optional[float]:
        """
        获取当前测距值(mm)
        重写基类方法
        
        Returns:
            float: 测距值(mm)，获取失败返回None
        """
        if not self.is_connected:
            logger.warning("传感器未连接")
            return None
            
        try:
            if self.ser and self.ser.in_waiting > 0:
                raw_data = b''
                while len(raw_data) < 195:
                    # 读取串口中所有可用的数据
                    raw_data += self.ser.read(self.ser.in_waiting)
                    time.sleep(0.01)  # 可以加一些延时，避免CPU占用过高
                
                # 确保读取到完整的195字节
                if len(raw_data) >= 195:
                    data = list(bytearray(raw_data[:195]))  # 取前195字节，防止溢出
                    measurements = parse_data(data)
                    
                    if measurements and len(measurements) > 0:
                        # 更新最新测距值（毫米）
                        with self.lock:
                            self.distance = measurements[0][0]  # 已经是毫米单位
                        return self.distance
            return None
        except Exception as e:
            logger.error(f"获取STP23L距离失败：{str(e)}")
            return None
    
    def start_ranging(self):
        """开始测距"""
        if not self.is_connected:
            if not self.open():
                return False
        
        if self.running:
            return True  # 已在运行
        
        self.running = True
        self.thread = threading.Thread(target=self._ranging_loop, daemon=True)
        self.thread.start()
        return True
    
    def stop_ranging(self):
        """停止测距"""
        self.running = False
        if self.thread:
            self.thread.join(timeout=1)
        return True
    
    def _ranging_loop(self):
        """测距循环"""
        while self.running:
            try:
                if self.ser.in_waiting > 0:
                    raw_data = b''
                    while len(raw_data) < 195 and self.running:
                        # 读取串口中所有可用的数据
                        raw_data += self.ser.read(self.ser.in_waiting)
                        time.sleep(0.01)  # 可以加一些延时，避免CPU占用过高
                    
                    # 确保读取到完整的195字节
                    if len(raw_data) >= 195 and self.running:
                        data = list(bytearray(raw_data[:195]))  # 取前195字节，防止溢出
                        measurements = parse_data(data)
                        
                        if measurements and len(measurements) > 0:
                            # 更新最新测距值（毫米）
                            with self.lock:
                                self.distance = measurements[0][0]  # 已经是毫米单位
                
                time.sleep(0.1)  # 控制读取频率
            except Exception as e:
                logger.error(f"STP23L测距异常: {e}")
                time.sleep(0.5)  # 出错时等待更长时间
    
    def add_measurement(self, distance: float) -> None:
        """
        添加测量值到列表中，并限制最大测量次数
        
        Args:
            distance: 测量距离值（毫米）
        """
        self.measurements.append(distance)
        
        # 如果超过最大测量次数，移除最早的测量值
        if len(self.measurements) > self.max_measurements:
            self.measurements.pop(0)
    
    def calculate_statistics(self) -> Dict[str, Union[float, int]]:
        """
        计算测量数据的统计信息
        
        Returns:
            包含各种统计值的字典
        """
        if not self.measurements:
            return {}
        
        measurements_array = np.array(self.measurements)
        
        # 1. 计算平均值
        mean_value = np.mean(measurements_array)
        
        # 2. 对测量结果排序并找出中位数
        sorted_measurements = np.sort(measurements_array)
        median_value = np.median(sorted_measurements)
        
        # 3. 计算方差
        variance = np.var(measurements_array)
        
        # 4. 计算去除最大最小值后的平均值
        if len(measurements_array) >= 3:
            trimmed_array = sorted_measurements[1:-1]  # 去除最大最小值
            trimmed_mean = np.mean(trimmed_array)
        else:
            trimmed_mean = mean_value
        
        # 5. 找出与平均值相差最大和最小的值
        differences = np.abs(measurements_array - mean_value)
        max_diff_idx = np.argmax(differences)
        min_diff_idx = np.argmin(differences)
        
        max_diff_value = measurements_array[max_diff_idx]
        min_diff_value = measurements_array[min_diff_idx]
        max_diff_amount = differences[max_diff_idx]
        min_diff_amount = differences[min_diff_idx]
        
        return {
            'mean': mean_value,
            'median': median_value,
            'variance': variance,
            'trimmed_mean': trimmed_mean,
            'max_diff_value': max_diff_value,
            'max_diff_amount': max_diff_amount,
            'min_diff_value': min_diff_value,
            'min_diff_amount': min_diff_amount,
            'count': len(self.measurements)
        }
    
    def print_measurement_table(self) -> None:
        """打印测量数据表格和统计信息"""
        if not self.measurements:
            print("没有测量数据")
            return
        
        # 打印表头
        print("\n" + "="*80)
        print("STP23L 测距数据统计结果")
        print("="*80)
        
        # 打印所有测量数据
        print("\n测量数据 (毫米):")
        print("-"*50)
        for i, measurement in enumerate(self.measurements, 1):
            print(f"测量 {i:2d}: {measurement:.2f}")
        
        # 计算并打印统计信息
        stats = self.calculate_statistics()
        if stats:
            print("\n统计信息:")
            print("-"*50)
            print(f"测量次数: {stats['count']}")
            print(f"平均值: {stats['mean']:.2f} 毫米")
            print(f"中位数: {stats['median']:.2f} 毫米")
            print(f"方差: {stats['variance']:.2f}")
            print(f"去除最大最小值后的平均值: {stats['trimmed_mean']:.2f} 毫米")
            print(f"与平均值相差最大的值: {stats['max_diff_value']:.2f} 毫米 (相差: {stats['max_diff_amount']:.2f})")
            print(f"与平均值相差最小的值: {stats['min_diff_value']:.2f} 毫米 (相差: {stats['min_diff_amount']:.2f})")
        
        print("="*80)
    
    def export_to_excel(self, filename: str) -> bool:
        """
        将测量数据和统计信息导出到Excel文件
        
        Args:
            filename: Excel文件名
            
        Returns:
            bool: 导出是否成功
        """
        try:
            # 如果没有测量数据，返回False
            if not self.measurements:
                logger.error("没有测量数据可导出")
                return False
                
            # 创建一个Excel写入器
            with pd.ExcelWriter(filename, engine='openpyxl') as writer:
                # 创建测量数据工作表
                measurements_df = pd.DataFrame({
                    '测量序号': range(1, len(self.measurements) + 1),
                    '测量值(毫米)': self.measurements
                })
                measurements_df.to_excel(writer, sheet_name='测量数据', index=False)
                
                # 创建统计信息工作表
                stats = self.calculate_statistics()
                if stats:
                    stats_data = {
                        '统计项': ['测量次数', '平均值(毫米)', '中位数(毫米)', '方差', 
                                 '去除最大最小值后的平均值(毫米)', 
                                 '与平均值相差最大的值(毫米)', '最大差值(毫米)',
                                 '与平均值相差最小的值(毫米)', '最小差值(毫米)'],
                        '数值': [
                            stats['count'],
                            stats['mean'],
                            stats['median'],
                            stats['variance'],
                            stats['trimmed_mean'],
                            stats['max_diff_value'],
                            stats['max_diff_amount'],
                            stats['min_diff_value'],
                            stats['min_diff_amount']
                        ]
                    }
                    stats_df = pd.DataFrame(stats_data)
                    stats_df.to_excel(writer, sheet_name='统计信息', index=False)
                
                # 创建排序数据工作表
                sorted_measurements = sorted(self.measurements)
                sorted_df = pd.DataFrame({
                    '排序序号': range(1, len(sorted_measurements) + 1),
                    '排序后的测量值(毫米)': sorted_measurements
                })
                sorted_df.to_excel(writer, sheet_name='排序数据', index=False)
            
            return True
        except Exception as e:
            logger.error(f"导出Excel失败: {e}")
            return False


if __name__ == "__main__":
    # 设置串口参数
    import sys
    if sys.platform == 'win32':
        port = 'COM3'  # Windows系统下的串口号，根据实际情况修改
    else:
        port = '/dev/ttyACM0'  # Linux系统下的串口号，根据实际情况修改
    baudrate = 230400  # 波特率
    
    # 执行测量
    read_serial_data()